In [1]:
import sympy
import sympy.physics.mechanics as me
import scipy.integrate
import scipy.optimize
%pylab inline
import matplotlib.animation
# video animation
from tempfile import NamedTemporaryFile
from IPython.display import HTML
sympy.init_printing()
Define symbols.
In [2]:
t, theta_a, Q_a, theta_b, Q_b, theta_c, Q_c, l_a, l_b, l_c, l_d, I_a, I_b, I_c, m_a, m_b, m_c, g = \
sympy.symbols(r't, theta_a, Q_a, theta_b, Q_b, theta_c, Q_c, l_a, l_b, l_c, l_d, I_a, I_b, I_c, m_a, m_b, m_c, g')
Define frame and points.
In [3]:
frame_i = me.ReferenceFrame(name='i') # inertial frame
frame_a = frame_i.orientnew(newname='a', rot_type='Axis', amounts=(theta_a(t), frame_i.z)) # link a frame
frame_b = frame_a.orientnew(newname='b', rot_type='Axis', amounts=(theta_b(t), frame_a.z)) # link b frame
frame_c = frame_b.orientnew(newname='c', rot_type='Axis', amounts=(theta_c(t), frame_b.z)) # link b frame
Define bar a.
In [4]:
point_a = me.Point(name='point_a') # axis of rotation for link a
point_a.set_vel(frame=frame_i, value=0) # fixed in inertial frame
cm_a = point_a.locatenew(name='cm_a', value=l_a/2*frame_a.x) # center of mass location
cm_a.v2pt_theory(otherpoint=point_a, outframe=frame_i, fixedframe=frame_a)
bar_a = me.RigidBody(name='bar_a', masscenter=cm_a, frame=frame_a,
mass=m_a, inertia=(me.inertia(frame_a, ixx=0, iyy=0, izz=I_a), point_a))
Define bar b.
In [5]:
point_b = point_a.locatenew(name='point_b', value=l_a*frame_a.x) # point b
point_b.v2pt_theory(otherpoint=point_a, outframe=frame_i, fixedframe=frame_b)
cm_b= point_b.locatenew(name='cm_b', value=l_b/2*frame_b.x) # center of mass location
cm_b.v2pt_theory(otherpoint=point_b, outframe=frame_i, fixedframe=frame_b)
bar_b = me.RigidBody(name='bar_b', masscenter=cm_b, frame=frame_b,
mass=m_b, inertia=(me.inertia(frame_b, ixx=0, iyy=0, izz=I_b), point_b))
Define bar c.
In [6]:
point_c = point_b.locatenew('point_c', l_b*frame_b.x) # point c
point_c.v2pt_theory(otherpoint=point_b, outframe=frame_i, fixedframe=frame_c)
cm_c= point_c.locatenew('cm_c', l_c/2*frame_c.x) # center of mass location
cm_c.v2pt_theory(otherpoint=point_c, outframe=frame_i, fixedframe=frame_c)
bar_c = me.RigidBody(name='bar_c', masscenter=cm_c, frame=frame_c,
mass=m_c, inertia=(me.inertia(frame_c, ixx=0, iyy=0, izz=I_c), point_c))
Define the pinned point at the end of bar c.
In [7]:
point_d = point_c.locatenew('point_d', l_c*frame_c.x) # point d
List of rigid bodies.
In [8]:
rigid_body_list = [bar_a, bar_b, bar_c]
Find the constraint equations such that the end point is fixed.
In [9]:
coneq = (point_d.pos_from(point_a) - l_d*frame_i.x).express(frame_i).simplify()
coneq
Out[9]:
Take the derivative of the constraint equations for use in deriving the equations of motion.
In [10]:
coneq_diff = coneq.diff(t, frame_i).express(frame_i).simplify()
coneq_diff
Out[10]:
In [11]:
coneqs = [coneq.dot(frame_i.x).simplify(), coneq.dot(frame_i.y).simplify()]
coneqs_diff = [coneq_diff.dot(frame_i.x).simplify(), coneq_diff.dot(frame_i.y).simplify()]
Define the forces and moments acting on the system.
In [12]:
force_moment_list = [
#(cm_a, -m_a*g*frame_i.y), # weight on a, doing horizontal, so weight is cancelled
#(cm_b, -m_b*g*frame_i.y), # weight on b
#(cm_c, -m_c*g*frame_i.y), # weight on c
(frame_a, (3 - theta_a(t).diff(t))*frame_a.z), # moment from motor, FIXME, these are bogus,
# just setting it to something interesting, added a damping term
(point_c, 1*frame_a.y), # lift from wing, FIXME
(frame_c, -1*frame_a.y), # moment from wing, FIXME
]
We will not explicitly show the time dependendence for the states.
In [13]:
x_no_t_sub = {theta_a(t): theta_a, theta_b(t): theta_b, theta_c(t): theta_c,
theta_a(t).diff(t): Q_a, theta_b(t).diff(t): Q_b, theta_c(t).diff(t): Q_c,
Q_a(t):Q_a, Q_b(t):Q_b, Q_c(t):Q_c}
Find the Lagrangian.
In [14]:
L = me.Lagrangian(frame_i, *rigid_body_list)
L.subs(x_no_t_sub)
Out[14]:
In [15]:
q_list = [theta_a(t), theta_b(t), theta_c(t)]
lm = me.LagrangesMethod(q_list=q_list, Lagrangian=L, frame=frame_i, coneqs=coneqs_diff, forcelist=force_moment_list)
lm.form_lagranges_equations();
In [16]:
lm_mass_matrix_full = lm.mass_matrix_full.subs(x_no_t_sub)
lm_mass_matrix_full
Out[16]:
In [17]:
lm_forcing_full = lm.forcing_full.subs(x_no_t_sub)
lm_forcing_full
Out[17]:
In [18]:
km = me.KanesMethod(
frame=frame_i,
q_ind=[theta_a(t)],
q_dependent=[theta_b(t), theta_c(t)],
u_ind=[Q_a(t), Q_b(t), Q_c(t)],
kd_eqs=[theta_a(t).diff(t) - Q_a(t), theta_b(t).diff(t) - Q_b(t), theta_c(t).diff(t) - Q_c(t)],
configuration_constraints=coneqs,
)
km.kanes_equations(FL=force_moment_list, BL=rigid_body_list);
In [19]:
km_mass_matrix_full = km.mass_matrix_full.subs(x_no_t_sub)
km_mass_matrix_full
Out[19]:
In [20]:
#(km_mass_matrix_full - lm_mass_matrix_full).applyfunc(lambda expr: expr.expand().simplify())
In [21]:
km_forcing_full = km.forcing_full.subs(x_no_t_sub)
km_forcing_full
Out[21]:
In [22]:
#(km_mass_matrix_full - lm_mass_matrix_full).applyfunc(lambda expr: expr.expand().simplify())
This should be zero, possible error in setup fo Kane's/ Lagrange's method?
In [23]:
#forcing_remainder = (km_forcing_full - lm_forcing_full).applyfunc(lambda expr: expr.expand(trig=True).simplify())
#forcing_remainder
Constants for simulation. Link a is attached to a motor, so it has more inertia.
In [24]:
const = {I_a: 1, I_b: 0.01, I_c: 0.01, l_a: 0.1, l_b: 0.5, l_c: 0.2, l_d: 0.5, m_a:1, m_b:1, m_c:1, g: 9.8}
Get equations for point locations.
In [25]:
f_points = []
for point in [point_b, point_c, point_d]:
r = point.pos_from(point_a).subs(const).express(frame_a).simplify()
f_points.append(sympy.lambdify((theta_a(t), theta_b(t), theta_c(t)), [r.dot(frame_i.x), r.dot(frame_i.y)]))
Simulate using numerical integration, inverting at each step using the mass and forcing matrices.
In [26]:
x = sympy.DeferredVector('x')
u = sympy.DeferredVector('u')
x_vect = sympy.Matrix([theta_a, theta_b, theta_c, Q_a, Q_b, Q_c])
x_sub = {x_vect[i]: x[i] for i in range(len(x_vect))}
lm_f_mass_matrix = sympy.lambdify((x), lm_mass_matrix_full.subs(x_sub).subs(const))
lm_f_forcing_matrix = sympy.lambdify((x), lm_forcing_full.subs(x_sub).subs(const))
lm_f_sim = lambda t, x, u: lm_f_mass_matrix(x).I*lm_f_forcing_matrix(x)
In [27]:
km_f_mass_matrix = sympy.lambdify((x), km_mass_matrix_full.subs(x_sub).subs(const))
km_f_forcing_matrix = sympy.lambdify((x), km_forcing_full.subs(x_sub).subs(const))
km_f_sim = lambda t, x, u: km_f_mass_matrix(x).I*km_f_forcing_matrix(x)
In [28]:
%%timeit
lm_f_sim(0,[1,1,1,1,1,1,1,1],0)
In [29]:
%%timeit
km_f_sim(0,[1,1,1,1,1,1,1,1],0)
Define a function to initialize the 4 bar linkage based on the angle/ rate of the motor.
In [30]:
con_diff = coneq_diff.express(frame_i).simplify().subs(x_no_t_sub)
con_diff.dot(frame_i.x)**2 + con_diff.dot(frame_i.y)**2
Out[30]:
In [31]:
def initialize_4bar(theta_a_val, q_a_val, con_eq, coneq_diff):
con = coneq.express(frame_i).simplify().subs(x_no_t_sub)
con_diff = coneq_diff.express(frame_i).simplify().subs(x_no_t_sub)
constraint_violation_cost = sympy.lambdify(
(theta_a, theta_b, theta_c, Q_a, Q_b, Q_c), (con.dot(frame_i.x)**2 + con.dot(frame_i.y)**2 +
con_diff.dot(frame_i.x)**2 + con_diff.dot(frame_i.y)**2).subs(const))
res = scipy.optimize.minimize(lambda x: constraint_violation_cost(theta_a_val, x[0], x[1], q_a_val, x[2], x[3]), zeros(4))
return [theta_a_val, res['x'][0], res['x'][1], q_a_val, res['x'][2], res['x'][3], 0, 0]
In [32]:
def simulate(x0, f_sim, tf=10,dt=1.0/20.0):
sim = scipy.integrate.ode(f_sim)
sim.set_integrator('dopri5')
n_y = len(x0)
sim.set_initial_value(x0)
n_t = floor(tf/dt)
data = {'tf': tf, 'dt': dt, 't': zeros(n_t), 'y': zeros((n_t, n_y))}
i = 0
while sim.successful() and i < n_t:
sim.set_f_params(0)
sim.integrate(sim.t + dt)
data['t'][i] = sim.t
data['y'][i,:] = sim.y
i += 1
# note, should reset state to satisfy constraint exactly at end of integration, could use
# minimize as done in init.
return data
In [33]:
def plot_4bar(f_points, data):
n_t = len(data['t'])
a = np.zeros((n_t,2))
b = np.zeros((n_t,2))
c = np.zeros((n_t,2))
d = np.zeros((n_t,2))
for i in arange(0, n_t):
theta_a = data['y'][i,0]
theta_b = data['y'][i,1]
theta_c = data['y'][i,2]
b[i,:] = f_points[0](theta_a, theta_b, theta_c)
c[i,:] = f_points[1](theta_a, theta_b, theta_c)
d[i,:] = f_points[2](theta_a, theta_b, theta_c)
h1 = plot([a[:,0], b[:,0]], [a[:,1], b[:,1]], 'k-')
h2 = plot([b[:,0], c[:,0]], [b[:,1], c[:,1]], 'r-')
h3 = plot([c[:,0], d[:,0]], [c[:,1], d[:,1]], 'g-')
legend([h1[0], h2[0], h3[0]], ['ab', 'bc','cd'], loc='best')
Run the simulation and plot the result.
In [34]:
x0 = initialize_4bar(1.0, 0.0, coneq, coneq_diff)
data = simulate(x0, lm_f_sim, tf=10.0)
figure(figsize=(15,5))
plot_4bar(f_points, data)
In [35]:
plt.rcParams['lines.linewidth'] = 2
plt.rcParams['font.size'] = 12
plt.rcParams['animation.writer'] = 'avconv'
plt.rcParams['animation.codec'] = 'libx264'
In [36]:
def anim_to_html(anim):
plt.close(anim._fig)
if not hasattr(anim, '_encoded_video'):
with NamedTemporaryFile(suffix='.mp4') as f:
anim.save(f.name, fps=20, extra_args=['-vcodec', 'libx264'])
video = open(f.name, "rb").read()
anim._encoded_video = video.encode("base64")
return """<video controls>
<source src="data:video/x-m4v;base64,{0}" type="video/mp4">
Your browser does not support the video tag.
</video>""".format(anim._encoded_video)
matplotlib.animation.Animation._repr_html_ = anim_to_html
In [37]:
def anim_4bar(data, fps=20, rate=1.0):
fig = plt.figure(figsize=(10,5))
ax = fig.gca()
n_t = len(data['t'])
a = np.zeros((n_t,2))
b = np.zeros((n_t,2))
c = np.zeros((n_t,2))
d = np.zeros((n_t,2))
t = data['t']
tf = data['tf']
dt = data['dt']
if (fps > 1.0/dt):
raise IOError('Data dt lower than fps, lower fps')
for i in arange(0, n_t):
theta_a = data['y'][i,0]
theta_b = data['y'][i,1]
theta_c = data['y'][i,2]
b[i,:] = f_points[0](theta_a, theta_b, theta_c)
c[i,:] = f_points[1](theta_a, theta_b, theta_c)
d[i,:] = f_points[2](theta_a, theta_b, theta_c)
def update(i):
ax.cla()
h1 = ax.plot([a[i,0], b[i,0]], [a[i,1], b[i,1]], 'k-')
h2 = ax.plot([b[i,0], c[i,0]], [b[i,1], c[i,1]], 'r-')
h3 = ax.plot([c[i,0], d[i,0]], [c[i,1], d[i,1]], 'g-')
ax.legend([h1[0], h2[0], h3[0]], ['ab', 'bc','cd'], loc=3)
ax.axis([-max(abs(b[:,0])), max(abs(c[:,0])), -max(abs(c[:,1])), max(abs(c[:,1]))])
ax.text((a[0,0] + d[0,0])*0.5, 0, 't: {:f}'.format(t[i,]))
return matplotlib.animation.FuncAnimation(fig, update, frames=int(tf*fps/rate))
In [38]:
anim_4bar(data)
Out[38]:
If we neglect the mass of the bars, the dynamics are greatly simplified. This would be a valid assumption if the motor inertia and the forces are much larger than the inertial forces/moments of the bars. Here we assume that the motor can be modelled as a rigid body attached at point a.
In [39]:
motor = me.RigidBody('motor', point_a, frame_a, m_a, (me.inertia(frame_a, ixx=0, iyy=0, izz=I_a), point_a))
L_simp = me.Lagrangian(frame_i, motor)
L_simp
Out[39]:
In [40]:
lm_simp = me.LagrangesMethod(
L_simp, q_list=(theta_a(t), theta_b(t), theta_c(t)),
coneqs=coneqs_diff, forcelist=force_moment_list, frame=frame_i)
lm_simp.form_lagranges_equations();
In [41]:
lm_simp_mass_matrix_full = lm_simp.mass_matrix_full.subs(x_no_t_sub)
lm_simp_mass_matrix_full
Out[41]:
In [42]:
lm_simp_forcing_full = lm_simp.forcing_full.subs(x_no_t_sub)
lm_simp_forcing_full
Out[42]:
For this simplified model, it is now possible to analytically take the inverse of the mass matrix. This allows dynamic inversion type controllers and also quicker simulation if the simplifying assumptions are valid.
In [43]:
lm_simp_rhs = lm_simp.rhs().subs(x_no_t_sub)
lm_simp_rhs
Out[43]:
In [44]:
lm_simp_f = sympy.lambdify(x, lm_simp_rhs.subs(x_no_t_sub).subs(x_sub).subs(const))
lm_simp_f_sim = lambda t, x, u: lm_simp_f(x)
In [45]:
%%timeit
lm_simp_f_sim(0,[1,1,1,1,1,1,1,1],0)
In [46]:
x0 = initialize_4bar(1.0, 0.0, coneq, coneq_diff)
data_simp = simulate(x0, lm_simp_f_sim, tf=10.0)
figure(figsize=(15,5))
plot_4bar(f_points, data_simp)
In [47]:
anim_4bar(data_simp)
Out[47]:
In [48]:
figure(figsize(15,5))
h1 = plot(data_simp['t'], data_simp['y'][:,2])
h2 = plot(data['t'], data['y'][:,2])
legend([h1[0], h2[0]], ['simplified', 'complex'])
Out[48]:
In [49]:
def generate_matlab(expr):
return str(expr.subs(x_no_t_sub)).replace('**','^').replace(r'Matrix([[','[').\
replace(r'], [',';\n').replace(']])','];').replace(r'\dot{\theta}','dtheta')
In [50]:
print generate_matlab(lm.mass_matrix_full)
print generate_matlab(lm.forcing_full)